﻿using System;
using System.Collections.Generic;
using System.Text;
using ElteIk.RoboSoccer.Core.Physics;

namespace ElteIk.RoboSoccer.DavidTactics
{
    class EngineDirector
    {
        private float[] left;
        private float[] right;
        private float[][] ones;

        public EngineDirector()
        {
            left = new float[15];
            right = new float[15];
            
            ones = new float[15][];
            for (int i = 0; i < 15; ++i)
            {
                ones[i] = new float[15];
                ones[i][i] = 1;
            }

            Random rand = new Random();

            for (int i = 0; i < 15; ++i)
            {
                left[i] = (float)(0.05 * rand.NextDouble());
                right[i] = (float)(0.05 * rand.NextDouble());
            }

            left[10] = -0.01f;
            right[10] = 0.005f;
        }

        public float LeftEngine(float r1, float r2, float s1, float s2)
        {
            return Calculate(r1, r2, s1, s2, left);
        }

        public float RightEngine(float r1, float r2, float s1, float s2)
        {
            return Calculate(r1, r2, s1, s2, right);
        }

        public float Calculate(float r1, float r2, float s1, float s2, float[] coefficients)
        {
            return
                coefficients[0] * r1 * r1 +
                coefficients[1] * r2 * r2 +
                coefficients[2] * s1 * s1 +
                coefficients[3] * s2 * s2 +
                coefficients[4] * r1 * r2 +
                coefficients[5] * r1 * s1 +
                coefficients[6] * r1 * s2 +
                coefficients[7] * r2 * s1 +
                coefficients[8] * r2 * s2 +
                coefficients[9] * s1 * s2 +
                coefficients[10] * r1 +
                coefficients[11] * r2 +
                coefficients[12] * s1 +
                coefficients[13] * s2 +
                coefficients[14];
        }

        public void Learn(List<Variables> actions, float alpha, float reward)
        {
            for (int i = 0; i < actions.Count-1; ++i)
            {
                Variables current = actions[i];
                Variables next = actions[i + 1];
                float cr1 = current.r1;
                float cr2 = current.r2;
                float cs1 = current.s1;
                float cs2 = current.s2;
                float nr1 = next.r1;
                float nr2 = next.r2;
                float ns1 = next.s1;
                float ns2 = next.s2;

                // Left engine.
                float luwi = LeftEngine(cr1, cr2, cs1, cs2);
                float luwj = LeftEngine(nr1, nr2, ns1, ns2);
                float qLeft = alpha * (reward + luwj - luwi);

                for (int j = 0; j < 15; ++j)
                {
                    float w = Calculate(cr1, cr2, cs1, cs2, ones[j]);
                    left[j] += qLeft * w;
                }
                
                // Right engine.
                float ruwi = RightEngine(cr1, cr2, cs1, cs2);
                float ruwj = RightEngine(nr1, nr2, ns1, ns2);
                float qRight = alpha * (reward + ruwj - ruwi);

                for (int j = 0; j < 15; ++j)
                {
                    float w = Calculate(cr1, cr2, cs1, cs2, ones[j]);
                    right[j] += qRight * w;
                }
            }
        }
    }
}